
#include "crowd_dynamics.h"
#include "pmodel.h"
#include "3d_primitives.h"

using namespace WtfEngine;

#define CLAUSTROPHOBIA	1

// Sampling region of discomfort (where r = 1)
const tScalar DECAY_RATE = 0.025;

const tScalar BASE_SPEED = 1;

const int	  NUM_CHOICES = 7;
const tScalar MAX_DEVIATION = M_PI / 2;
const tScalar DISCOMFORT_LIMIT = 20.0;

ICrowdMember::tCrowdList	ICrowdMember::gCrowdList;


/**** Crowd Member Interface *****/

void ICrowdMember::Init() {
	gCrowdList.push_back(this);
};
void ICrowdMember::Destroy() {
	gCrowdList.remove(this);
};

tScalar ICrowdMember::_getDiscomfort(const IMobileActor::tRef& rActor, const tPosition& vPos) {
	ICrowdMember::tCrowdList::const_iterator	i;
	tPosition rel;
	tScalar d = 0;
	
	for(i = gCrowdList.begin(); i != gCrowdList.end(); i++)
	{
		if((*i) != dynamic_cast<ICrowdMember*>(&*rActor))
		{
			// Our position relative to teh object.
			// Only consider objects in our field of view, i.e.
			// the direction we are travelling
			rel = vPos - (*i)->getPosition();
			
#			if(CLAUSTROPHOBIA == 0)
				if(rel.dot(rActor->getVelocity()) < 0)
#			endif
					d += (*i)->getDiscomfort(rActor, rel);
		};
	};

	return d;
};



/**** Crowd Behaviour ****/

bool Behaviours::CrowdBehaviour::Evaluate(const IMobileActor::tRef &rActor, const tPosition& vGoal, tDirection &vDesiredVelocity) {
	unsigned	n, m;
	tVector		vels[NUM_CHOICES];
	tVector		rel;
	tScalar		discomforts[NUM_CHOICES];
	tScalar		a;
	

	//
	// Enumerate the positions we can choose, based upon small
	// angular deviations in our velocity in the XZ plane (TODO: 3D?)
	//

	rel = (vGoal - rActor->getPosition()); // + rActor->getVelocity();

	m = (NUM_CHOICES + 1) / 2; // Median value
	for(n = 0; n < NUM_CHOICES; n++){
		a = (m - n) * (MAX_DEVIATION / (NUM_CHOICES / 2));
		vels[n][0] = (rel[0] * cos(a)) - (rel[1] * sin(a));
		vels[n][1] = (rel[1] * cos(a)) + (rel[0] * sin(a));
		vels[n][2] = rel[2];

		discomforts[n] = ICrowdMember::_getDiscomfort(rActor, rActor->getPosition() + vels[n]);
	};


	//
	// Now find the one with the lowest value, and set our velocity to it
	//

	for(n = 0; n < NUM_CHOICES; n++){
		if(discomforts[n] < discomforts[m]) // (m initially median)
			m = n;
	};

	vDesiredVelocity = vels[m];
	return true;
};



/**** Crowd Member ****/

CrowdMember::CrowdMember(const WtfGraphics::GraphicsData::tCacheRef& rModel,
			const PhysicalModel::tCacheRef& rPhys, tScalar mq,
			tScalar intimidation, tScalar resistance)
: Physical(tCreationOptions(), rModel, rPhys), mIntimidation(intimidation), mResistance(resistance)
{
	ICrowdMember::Init();
};

void CrowdMember::Test()
{
	int		i;
	tVector	vel(100, 0, 0), pos(-200,300,0);
	CrowdMember * p;

	WtfGraphics::GraphicsData::tCacheRef	rModel(new WtfGraphics::Primitives::Oblong(10.0, 10.0, 10.0));
	//WtfGraphics::GraphicsData::tRef	rModel(new WtfGraphics::Primitives::Sphere(10.0, 16, 16));
	PhysicalModel::tCacheRef	rPhys(new PhysicalModels::Cube("", 10.0));

	// Create two "walls" of objects, travelling in opposite directions
	for(i = 0; i < 20; i++){
		p = new CrowdMember(rModel, rPhys, 1.0);
		p->setVelocity(vel);
		p->setPosition(pos + tVector(rand() % 100, rand() % 100, 0));
		if(i == 5)
			pos[1] -= 100;
		else
			pos[1] -= 30;

		rModel = new WtfGraphics::Primitives::Oblong(10.0, 10.0, 10.0);
		//rModel = new WtfGraphics::Primitives::Sphere(100.0, 16, 16);
	};

	vel = -vel;
	pos[0] = 200;
	for(i = 0; i < 20; i++){
		p = new CrowdMember(rModel, rPhys, 1.0);
		p->setVelocity(vel);
		p->setPosition(pos + tVector(rand() % 100, rand() % 100, 0) );
		
		pos[1] += 30;

		rModel = new WtfGraphics::Primitives::Oblong(10.0, 10.0, 10.0);
		//rModel = new WtfGraphics::Primitives::Sphere(100.0, 16, 16);
	};

	for(i = 0; i < 30; i++){
		p = new CrowdMember(rModel, rPhys, 1.0, 50);
		p->setVelocity(vel);
		p->setPosition(pos + tVector(rand() % 100, rand() % 100, 0));
	

		rModel = new WtfGraphics::Primitives::Oblong(10.0, 10.0, 10.0);
	};
};

tScalar CrowdMember::getDiscomfort(const IMobileActor::tRef& rActor, const tPosition& pos) const 
{
	tScalar arcRatio, distRatio, phi, theta2 = 0.5 * M_PI * M_PI;
	
	//
	// Calculate ratio based upon distance of position from the
	// object.
	//

	distRatio = exp(-DECAY_RATE * pos.lengthSqr());

	//
	// Calculate ratio based upon deviance of the position from
	// this object's velocity vector
	//

	phi = acos(this->getVelocity().dot(pos) / (this->getVelocity().length()
			* pos.length()));

	arcRatio = 1 - ((phi * phi) / theta2);

	//
	// Now combine ratios to give discomfort val.
	//

	return (arcRatio < 0 ? -(distRatio / mIntimidation)
	: (this->mIntimidation * distRatio * arcRatio));
};

void CrowdMember::Update() {
	
};

